#include <moveit/move_group_interface/move_group_interface.h>
#include <moveit/planning_scene_interface/planning_scene_interface.h>

#include <moveit_msgs/DisplayRobotState.h>
#include <moveit_msgs/DisplayTrajectory.h>

#include <moveit_msgs/AttachedCollisionObject.h>
#include <moveit_msgs/CollisionObject.h>

#include <moveit_visual_tools/moveit_visual_tools.h>

// #include <math.h>

bool move_status = true;

// geometry_msgs::Pose start_pose2;
// start_pose2.orientation.y = 0.70711;
// start_pose2.orientation.w = 0.70711;
// start_pose2.position.x = 0.30;
// start_pose2.position.y = 0.0;
// start_pose2.position.z = 0.10;

// geometry_msgs::Pose target_pose;
// target_pose.orientation.w = 1.0;
// target_pose.position.x = 0.18;
// target_pose.position.y = -0.12;
// target_pose.position.z = 0.25;


bool Cartesian_Move(geometry_msgs::Pose start_pose,geometry_msgs::Pose target_pose,int interval_num);


/*笛卡尔运动规划的ROS服务端*/
int main(int argc, char** argv)
{
    // cout << "等待接收******" << endl;
    // //ROS节点初始化
    // ros::init(argc, argv, "cath_server");
    // //创建节点句柄
    // ros::NodeHandle node_handle;
    // // 创建服务端
    // // ros::ServiceServer server = n.advertiseService("dofbot_kinemarics", Cartesian_Move);
    // // 阻塞
    // ros::spin();
    return 0;

}

// bool Cartesian_Move(dofbot_info::kinemaricsRequest &request,dofbot_info::kinemaricsResponse &response)
// {
//   // @ start_pose:起始位置
//   // @ target_pose:开始位置
//   //geometry_msgs::Pose start_pose,geometry_msgs::Pose target_pose,int interval_num = 3
//     static const std::string PLANNING_GROUP = "gluon";
//     //路径规划
//     moveit::planning_interface::MoveGroupInterface move_group(PLANNING_GROUP);
//     //障碍规划
//     moveit::planning_interface::PlanningSceneInterface planning_scene_interface;
//     const moveit::core::JointModelGroup* joint_model_group =
//         move_group.getCurrentState()->getJointModelGroup(PLANNING_GROUP);

    

//     float interval_x,interval_y,interval_z;
//     std::vector<geometry_msgs::Pose> waypoints;

//     interval_x = (target_pose.position.x - start_pose.position.x)/interval_num;
//     interval_y = (target_pose.position.y - start_pose.position.y)/interval_num;
//     interval_z = (target_pose.position.z - start_pose.position.z)/interval_num;

//     geometry_msgs::Pose temp_pose = start_pose;
//     for(int i=0;i<interval_num;i++)
//     {
//         temp_pose.position.x += interval_x;
//         temp_pose.position.y += interval_y;
//         temp_pose.position.z += interval_z;
//         waypoints.push_back(temp_pose);
//     }

//     moveit_msgs::RobotTrajectory trajectory;
//     const double jump_threshold = 0.0;
//     const double eef_step = 0.01;
//     double fraction = move_group.computeCartesianPath(waypoints, eef_step, jump_threshold, trajectory);

//     move_group.execute(trajectory);
//     ros::Duration(2).sleep();
  
//     return true;

// }
